#include "angle_position.h"


void Angle_Position_Calc(void)
{
    float spdtmp = 0;
		int32_t postemp = 0;
#if MOTOR_VERSION == MC300
    if((stMotorPara.LastEncodedata_18bit > (uint16_t)HighSideMaxEncoderCnt)&&(stMotorPara.Encodedata_18bit < (uint16_t)LowSideMinEncoderCnt))
        stMotorPara.Multiturn++;
    else if((stMotorPara.Encodedata_18bit > (uint16_t)HighSideMaxEncoderCnt)&&(stMotorPara.LastEncodedata_18bit < (uint16_t)LowSideMinEncoderCnt))
        stMotorPara.Multiturn--;
    stMotorPara.LastEncodedata_18bit = stMotorPara.Encodedata_18bit;
    stMotorPara.AbsolutePos = (int32_t)stMotorPara.Multiturn * (int32_t)MOTOR_EncoderResolution + (int32_t)stMotorPara.Encodedata_18bit;
#elif MOTOR_VERSION == XIEBO500W
    stMotorPara.AbsolutePos = (int32_t)stMotorPara.Multiturn * (int32_t)MOTOR_EncoderResolution + (int32_t)stMotorPara.absEncodedata;
#endif

    spdtmp = (float)(stMotorPara.AbsolutePos-stMotorPara.LastAbsolutePos)*stMotorPara.SpeedRatio;

    if(fabsf(spdtmp) < 10000)
        stMotorSate.SpdReal = spdtmp;

    LP_FILTER(stMotorSate.SpdRealFliter,stMotorSate.SpdReal,1);

    stMotorPara.LastAbsolutePos = stMotorPara.AbsolutePos;

#if MOTOR_VERSION == MC300
    postemp = stMotorPara.AbsolutePos>>2;
    stMotorSate.PosReal = postemp - stMotorPara.PosZero;
#else

    stMotorSate.PosReal = stMotorPara.AbsolutePos - stMotorPara.PosZero;
#endif
    LP_FILTER(stMotorSate.PosRealFliter,stMotorSate.PosReal,0.1f);

}


void CalcOutEncoderValue(void)
{

}
